/*
	TOPCAT Payload Software, for use in the UKube-1 CubeSat mission.
	
	http://www.bath.ac.uk/elec-eng/invert/topcat.html
	
	Written by:
	Mark Jessop, University of Adelaide
	Talini Pinto Jayawardena, University of Bath
	
	Uses extracts from source code provided by Clyde Space for I2C communications.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    For a full copy of the GNU General Public License, 
    see <http://www.gnu.org/licenses/>.
*/
#include <Mega_SPI.h>
#include <spieeprom.h>
#include <EEPROM.h>

#define EEPROM_VAR_PAGE	511

unsigned int EEPROM_HEAD = 0;
unsigned int EEPROM_TAIL = 0;


// Copy a page into the buffer.
uint8_t EEPROM_read_page(){
  // if the head isn't ahead of the tail, we don't have any characters
  if (EEPROM_HEAD == EEPROM_TAIL) {
    return 0;
  } else {
  
  	EEPROM_copy_from_address((long)((long)EEPROM_TAIL * (long)256), 256);
    EEPROM_TAIL = (unsigned int)((EEPROM_TAIL + 1) % MAX_PAGES);
    return 1;
  }
}

// Copy a page into the buffer.
uint8_t EEPROM_buffer_page(){

	if (EEPROM_HEAD == EEPROM_TAIL) {
		return 0;
	}
  
	// Wipe the buffer.
	eeprom_next_read_page[0] = 0xDE;
	eeprom_next_read_page[1] = 0xAD;
	eeprom_next_read_page[2] = 0xBE;
	eeprom_next_read_page[3] = 0xEF;
	for(int m = 4; m < 256; m++)
	{
		eeprom_next_read_page[m] = 0x00;    // clear buffer
	}
  	kick_watchdog();
  	
	// if the head isn't ahead of the tail, we don't have any characters
	Serial.print("READING FROM EEPROM ADDR:");
	long addr = (long)((long)EEPROM_TAIL * (long)256);
	Serial.println(addr, DEC);
	eeprom_next_read_page[0] = storage.start_read(addr);
	//Serial.print(eeprom_next_read_page[0],HEX);
	//Serial.print(" ");
	for(int i = 1; i<256; i++){
		eeprom_next_read_page[i] = storage.continue_read();
	//	Serial.print(eeprom_next_read_page[i],HEX);
	//	Serial.print(" ");
	
	if(i==128){kick_watchdog();}
	
	}
	kick_watchdog();
	storage.end_read();
	
	// We should increment the TAIL counter AFTER we send the packet.
	// This way we won't loose any packets if we get shutdown before sending.
	//EEPROM_TAIL = (unsigned int)((EEPROM_TAIL + 1) % MAX_PAGES);
	
	bitSet(EEPROM_FLAGS,EEPROM_PAGE_READY);
	return 1;
  
}

void EEPROM_increment_tail(){
	EEPROM_mark_page_read(EEPROM_TAIL);
	EEPROM_TAIL = (unsigned int)((EEPROM_TAIL + 1) % MAX_PAGES);
}

void EEPROM_mark_page_read(unsigned int pageno){
	long page_addr = (long)pageno * 256;
	uint8_t page_msb = storage.read_byte(page_addr);
	bitSet(page_msb,7); // Set the MSB high.
	if(storage.write(page_addr, page_msb) == 0){
		//bitSet(PAYLOAD_REQUEST_FLAGS, REQUEST_RESTART); // Not sure if we want to request a restart at this point.
	}
}

uint8_t EEPROM_write_page(){
	unsigned int i = (unsigned int)((EEPROM_HEAD + 1) % MAX_PAGES);
	
	if (i != EEPROM_TAIL) {
		if( storage.write(EEPROM_HEAD * 256, eeprom_write_buffer, 256) == 0){
			bitSet(PAYLOAD_REQUEST_FLAGS, REQUEST_RESTART);
		}
		EEPROM_HEAD = i;
		return 1;
	}else{
		return 0;
	}
	//EEPROM_update_data_waiting();
}

// Used if we want to set a packet available for reading again.
void EEPROM_undo_read(){
	EEPROM_TAIL = (unsigned int)((EEPROM_TAIL-1) % MAX_PAGES);
}

unsigned int EEPROM_used()
{
  return (unsigned int)((MAX_PAGES + EEPROM_HEAD - EEPROM_TAIL) % MAX_PAGES);
}

void EEPROM_flush()
{
  EEPROM_HEAD = 0; 
  EEPROM_TAIL = 0;
  EEPROM_update_data_waiting();
}

void EEPROM_copy_from_address(long address, int len){
	eeprom_buffer[0] = storage.start_read(address);
	
	for(int i = 1; i<len; i++){
		eeprom_buffer[i] = storage.continue_read();
	}
	storage.end_read();
}

void EEPROM_update_data_waiting(){
	unsigned int temp_pages = EEPROM_used();
	PAYLOAD_DATA_WAITING = temp_pages;
}

void EEPROM_clear_write_buffer(void)
{
  int m;

  for(m = 0; m < 256; m++)
  {
    eeprom_write_buffer[m] = 0x00;    // clear txd_buffer[]
  }
}

void EEPROM_erase(){
	if(storage.chipErase() == 0){
		bitSet(PAYLOAD_REQUEST_FLAGS, REQUEST_RESTART);
	}
	EEPROM_flush();
}

void EEPROM_store_state(){
	uint8_t eeprom_data_store[6];
	eeprom_data_store[0] = (uint8_t)EEPROM_HEAD;
	eeprom_data_store[1] = (uint8_t)(EEPROM_HEAD>>8);
	eeprom_data_store[2] = (uint8_t)EEPROM_TAIL;
	eeprom_data_store[3] = (uint8_t)(EEPROM_TAIL>>8);
	unsigned int crc = crc16(0xFFFF, eeprom_data_store, 4);
	eeprom_data_store[4] = (uint8_t)(crc>>8);
	eeprom_data_store[5] = (uint8_t)crc;
	
	if(storage.write(EEPROM_VAR_PAGE * 256, eeprom_data_store, 6) == 0){
		bitSet(PAYLOAD_REQUEST_FLAGS, REQUEST_RESTART);
	}
		
	
	// Also write to the internal EEPROM
	for(int i = 0; i<6; i++){
		EEPROM.write(i,eeprom_data_store[i]);
	}

}

void EEPROM_restore_state(){
	uint8_t eeprom_data_store[6];
	uint8_t internal_eeprom_store[6];
	
	// Read data from External EEPROM.
	eeprom_data_store[0] = storage.start_read(EEPROM_VAR_PAGE * 256);
	
	for(int i = 1; i<6; i++){
		eeprom_data_store[i] = storage.continue_read();
	}
	storage.end_read();

	unsigned int crc = crc16(0xFFFF, eeprom_data_store, 4);
	
	if( ((uint8_t)crc == eeprom_data_store[5]) && ((uint8_t)(crc>>8) == eeprom_data_store[4]) ){
		// Restore HEAD and TAIL values.
		EEPROM_HEAD = (unsigned int) eeprom_data_store[0];
		EEPROM_HEAD |= (unsigned int) (eeprom_data_store[1]<<8);
		EEPROM_TAIL = (unsigned int) eeprom_data_store[2];
		EEPROM_TAIL |= (unsigned int) (eeprom_data_store[3]<<8);
		EEPROM_update_data_waiting();
		#if defined(SERIAL_DEBUG)
    		Serial.print("Restored EEPROM State from Ext EEPROM: HEAD=");
    		Serial.print(EEPROM_HEAD, DEC);
    		Serial.print(" TAIL=");
    		Serial.print(EEPROM_TAIL, DEC);
    		Serial.print(" USED=");
    		Serial.println(EEPROM_used(), DEC);
    	#endif
		
	}else{ // Try and read the data from the internal eeprom instead.
		#if defined(SERIAL_DEBUG)
    		Serial.print("External EEPROM Checksum Failure. Trying Internal EEPROM.");
    	#endif
    	
    	// Read from internal eeprom.
    	for(int i = 0; i<6; i++){
			internal_eeprom_store[i] = EEPROM.read(i);
		}
		// Check the checksum.
    	unsigned int crc = crc16(0xFFFF, internal_eeprom_store, 4);
		if( ((uint8_t)crc == internal_eeprom_store[5]) && ((uint8_t)(crc>>8) == internal_eeprom_store[4]) ){
			// Restore HEAD and TAIL values.
			EEPROM_HEAD = (unsigned int) internal_eeprom_store[0];
			EEPROM_HEAD |= (unsigned int) (internal_eeprom_store[1]<<8);
			EEPROM_TAIL = (unsigned int) internal_eeprom_store[2];
			EEPROM_TAIL |= (unsigned int) (internal_eeprom_store[3]<<8);
			EEPROM_update_data_waiting();
			#if defined(SERIAL_DEBUG)
				Serial.print("Restored EEPROM State from internal EEPROM: HEAD=");
				Serial.print(EEPROM_HEAD, DEC);
				Serial.print(" TAIL=");
				Serial.print(EEPROM_TAIL, DEC);
				Serial.print(" USED=");
				Serial.println(EEPROM_used(), DEC);
			#endif
		}else{
			#if defined(SERIAL_DEBUG)
				Serial.print("Internal EEPROM Checksum failure. Attempting to recover data.");
			#endif
    		// Cannot guarantee data from either eeprom is accurate. Extract required data
    		
    		
    		// Read sequence numbers of all packets, find head & tail values.
    		
    		
    		// Else, erase.
			EEPROM_erase();
			#if defined(SERIAL_DEBUG)
				Serial.println("Recovery Failed. EEPROM Erased.");
			#endif
		}
	}	
	
	// Wipe the checksum bytes. We want to make sure we don't restore old head/tail values.
	#if defined(SERIAL_DEBUG)
		Serial.println("Mangling checksum.");
	#endif
	EEPROM.write(4,0);
	EEPROM.write(5,0);
	eeprom_data_store[4] = 0x00;
	eeprom_data_store[5] = 0x00;
	storage.write(EEPROM_VAR_PAGE * 256, eeprom_data_store, 6);
}

void EEPROM_debug(){
	#if defined(SERIAL_DEBUG)
		Serial.print("HEAD ");
		Serial.println(EEPROM_HEAD, DEC);
		Serial.print("TAIL ");
		Serial.println(EEPROM_TAIL, DEC);
	#endif
}

uint8_t EEPROM_test(){
	uint8_t	test_value = 0xAA; // Rolled with a fair die, guaranteed to be random.
	long test_addr = (511 * 256) + 50; // 50 Bytes into the last page. May want to randomize this.
	
	if(storage.write(test_addr, test_value) == 0){
		//bitSet(PAYLOAD_REQUEST_FLAGS, REQUEST_RESTART); // Not sure if we want to request a restart at this point.
	}
	uint8_t test_read = storage.read_byte(test_addr);
	
	if(test_read == test_value){ 
		#if defined(SERIAL_DEBUG)
			Serial.println("EEPROM Test Passed.");
		#endif
	
		return 1; 
	}else{
		#if defined(SERIAL_DEBUG)
			Serial.println("EEPROM Test Failed.");
		#endif
		bitSet(EEPROM_FLAGS, EEPROM_BROKEN);
		return 0;
	}
}


